#include "manku_can_adapter.h"
#include "sensor_utils.hpp"
#include <linux/can.h>
#include <linux/can/raw.h>

using namespace sensor::radar;
using namespace sensor::utils;
MankuCanAdapter::MankuCanAdapter(std::unordered_map<std::string, std::shared_ptr<CanTransreceiver>> devs)
    : CanAdapter(devs, "Manku")
{
    using namespace std::placeholders;
    message_unpack_table_ = {
        { MC1_CR_RS_OBJ_FD_FR_FRAME_ID, std::bind(&MankuCanAdapter::onReceiverRS6bbFRInfo, this, _1, _2, _3) },
        { MC1_CR_RS_OBJ_FD_RR_FRAME_ID, std::bind(&MankuCanAdapter::onReceiverRS6abRRInfo, this, _1, _2, _3) },
        { MC1_CR_RS_STATUS_FR_FRAME_ID, std::bind(&MankuCanAdapter::onReceiverRS520FRInfo, this, _1, _2, _3) },
        { MC1_CR_RS_STATUS_RR_FRAME_ID, std::bind(&MankuCanAdapter::onReceiverRS510RRInfo, this, _1, _2, _3) },
    };
    std::map<uint16_t, unsigned int> canIdMap = {
        { MC1_FMR_RS_DETANDOBJ_COUNT_FRAME_ID, CAN_EFF_MASK },
        { MC1_CR_RS_DETANDOBJ_COUNT_RR_FRAME_ID, CAN_EFF_MASK },
        { MC1_CR_RS_DETANDOBJ_COUNT_FR_FRAME_ID, CAN_EFF_MASK },
        { MC1_FMR_RS_OBJ_FD_FRAME_ID, CAN_EFF_MASK },
        { MC1_CR_RS_OBJ_FD_FR_FRAME_ID, CAN_EFF_MASK },
        { MC1_CR_RS_OBJ_FD_RR_FRAME_ID, CAN_EFF_MASK }
    };
    can_devs_["can1"]->filter(canIdMap);
}

std::unordered_map<int, int> MankuCanAdapter::MankuObjectTypeMap = {
    { 0, 1 }, // 4轮车
    { 1, 102 }, // 2轮车
    { 2, 101 }, // 行人
    { 3, 0 }, // 未知
};

void MankuCanAdapter::onReceiveCanMessage(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    // RCLCPP_INFO(logger_, "ChuanSu Rx:%s %04x %d %s", dev.c_str(), fid, buffer.readableBytes(),
    //     encodeToHexString(buffer.toStringPiece().as_string()).c_str());

    auto iter = message_unpack_table_.find(fid);
    if (iter == message_unpack_table_.end()) {
        // RCLCPP_WARN(logger_, "No Handle to unpack frame %04x", fid);
        return;
    }

    iter->second(dev, fid, buffer);
}

void MankuCanAdapter::onReceiveGnssMessage(const interface::msg::Gnss::SharedPtr)
{
}

void MankuCanAdapter::onReceiveInformationMessage(const interface::msg::CarInformation msg)
{
    // RCLCPP_INFO(logger_, "onReceiveInformationMessage start");
    // 手动档位
    static std::unordered_map<int, int> gearsMkMap = {
        { 0, 0xa },
        { 1, 0x1 },
        { 2, 0x2 },
        { 3, 0x3 },
        { 4, 0x4 },
        { 5, 0x5 },
        { 6, 0x6 },
        { 7, 0xb },
        { 8, 0x2 },
    };
    // 自动档位
    static std::unordered_map<int, int> gearsAutoMkMap = {
        { 0, 0x0 },
        { 1, 0x1 },
        { 2, 0x2 },
        { 3, 0x3 },
        { 4, 0x4 },
        { 5, 0x5 },
        { 6, 0x6 },
        { 7, 0xb },
        { 8, 0xc },
    };
    {
        // 方向盘 0xe0
        mc1_fmr_sas_status_t safStatus { 0 };
        safStatus.sas_steer_wheel_angle = mc1_fmr_sas_status_sas_steer_wheel_angle_encode(msg.wheelangle);
        safStatus.sas_failure_sts = mc1_fmr_sas_status_sas_steer_wheel_angle_encode(1.0);
        uint8_t dstp[8];
        if (int ret = mc1_fmr_sas_status_pack(dstp, &safStatus, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", MC1_FMR_SAS_STATUS_FRAME_ID, buffer);
        }
    }
    {
        // ESC 0x125
        mc1_fmr_esc_status_t escStatus { 0 };
        escStatus.esc_vehicle_speed = mc1_fmr_esc_status_esc_vehicle_speed_encode(msg.speed);
        escStatus.esc_vehicle_speed_invalid = mc1_fmr_esc_status_esc_vehicle_speed_invalid_encode(1.0);
        uint8_t dstp[8];
        if (int ret = mc1_fmr_esc_status_pack(dstp, &escStatus, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", MC1_FMR_ESC_STATUS_FRAME_ID, buffer);
        }
    }
    {
        // 自动档位  0x165
        mc1_fmr_tcu_status_t tcuStatus { 0 };
        tcuStatus.tcu_current_gear_position = gearsAutoMkMap[msg.gearstate];
        uint8_t dstp[8];
        if (int ret = mc1_fmr_tcu_status_pack(dstp, &tcuStatus, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", MC1_FMR_VCU_MODE_CONTROL_FRAME_ID, buffer);
        }
    }
    {
        // 偏航角 0x130
        mc1_fmr_yrs1_t fmrYrs1 { 0 };
        fmrYrs1.yrs_yaw_rate = msg.yawrate;
        uint8_t dstp[8];
        if (int ret = mc1_fmr_yrs1_pack(dstp, &fmrYrs1, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", MC1_FMR_YRS1_FRAME_ID, buffer);
        }
    }
}

void MankuCanAdapter::fieldParsingfr01(interface::msg::Object& object, mc1_cr_rs_obj_fd_fr_t& objdf)
{
    object.source = 2;
    object.id = objdf.rs_obj00_id;
    object.x = mc1_cr_rs_obj_fd_fr_rs_obj00_y_pos_decode(objdf.rs_obj00_y_pos) * -1;
    object.y = mc1_cr_rs_obj_fd_fr_rs_obj00_x_pos_decode(objdf.rs_obj00_x_pos);
    object.y = 0;
    object.speedx = mc1_cr_rs_obj_fd_fr_rs_obj00_x_vel_absolute_decode(objdf.rs_obj00_x_vel_absolute);
    object.speedy = mc1_cr_rs_obj_fd_fr_rs_obj00_y_vel_absolute_decode(objdf.rs_obj00_y_vel_absolute);
    object.speed = std::hypot(object.speedx, object.speedy);
    object.heading = mc1_cr_rs_obj_fd_fr_rs_obj00_heading_decode(objdf.rs_obj00_heading);
    object.length = mc1_cr_rs_obj_fd_fr_rs_obj00_length_decode(objdf.rs_obj00_length);
    object.width = mc1_cr_rs_obj_fd_fr_rs_obj00_width_decode(objdf.rs_obj00_width);
    object.type = MankuObjectTypeMap[objdf.rs_obj00_type];
}

void MankuCanAdapter::fieldParsingfr02(interface::msg::Object& object, mc1_cr_rs_obj_fd_fr_t& objdf)
{
    object.source = 2;
    object.id = objdf.rs_obj00_id;
    object.x = mc1_cr_rs_obj_fd_fr_rs_obj01_y_pos_decode(objdf.rs_obj01_y_pos) * -1;
    object.y = mc1_cr_rs_obj_fd_fr_rs_obj01_x_pos_decode(objdf.rs_obj01_x_pos);
    object.y = 0;
    object.speedx = mc1_cr_rs_obj_fd_fr_rs_obj01_x_vel_absolute_decode(objdf.rs_obj01_x_vel_absolute);
    object.speedy = mc1_cr_rs_obj_fd_fr_rs_obj01_y_vel_absolute_decode(objdf.rs_obj01_y_vel_absolute);
    object.speed = std::hypot(object.speedx, object.speedy);
    object.heading = mc1_cr_rs_obj_fd_fr_rs_obj01_heading_decode(objdf.rs_obj01_heading);
    object.length = mc1_cr_rs_obj_fd_fr_rs_obj01_length_decode(objdf.rs_obj01_length);
    object.width = mc1_cr_rs_obj_fd_fr_rs_obj01_width_decode(objdf.rs_obj01_width);
    object.type = MankuObjectTypeMap[objdf.rs_obj01_type];
}

void MankuCanAdapter::fieldParsingrr01(interface::msg::Object& object, mc1_cr_rs_obj_fd_rr_t& objdf)
{
    object.source = 2;
    object.id = objdf.rs_obj00_id;
    object.x = mc1_cr_rs_obj_fd_rr_rs_obj00_x_pos_decode(objdf.rs_obj00_x_pos);
    object.y = mc1_cr_rs_obj_fd_rr_rs_obj00_y_pos_decode(objdf.rs_obj00_y_pos);
    object.y = 0;
    object.speedx = mc1_cr_rs_obj_fd_rr_rs_obj00_x_vel_absolute_decode(objdf.rs_obj00_x_vel_absolute);
    object.speedy = mc1_cr_rs_obj_fd_rr_rs_obj00_y_vel_absolute_decode(objdf.rs_obj00_y_vel_absolute);
    object.speed = std::hypot(object.speedx, object.speedy);
    object.heading = mc1_cr_rs_obj_fd_rr_rs_obj00_heading_decode(objdf.rs_obj00_heading);
    object.length = mc1_cr_rs_obj_fd_rr_rs_obj00_length_decode(objdf.rs_obj00_length);
    object.width = mc1_cr_rs_obj_fd_rr_rs_obj00_width_decode(objdf.rs_obj00_width);
    object.type = MankuObjectTypeMap[objdf.rs_obj00_type];
}
void MankuCanAdapter::fieldParsingrr02(interface::msg::Object& object, mc1_cr_rs_obj_fd_rr_t& objdf)
{
    object.source = 2;
    object.id = objdf.rs_obj00_id;
    object.x = mc1_cr_rs_obj_fd_rr_rs_obj01_x_pos_decode(objdf.rs_obj01_x_pos);
    object.y = mc1_cr_rs_obj_fd_rr_rs_obj01_y_pos_decode(objdf.rs_obj01_y_pos);
    object.y = 0;
    object.speedx = mc1_cr_rs_obj_fd_rr_rs_obj01_x_vel_absolute_decode(objdf.rs_obj01_x_vel_absolute);
    object.speedy = mc1_cr_rs_obj_fd_rr_rs_obj01_y_vel_absolute_decode(objdf.rs_obj01_y_vel_absolute);
    object.speed = std::hypot(object.speedx, object.speedy);
    object.heading = mc1_cr_rs_obj_fd_rr_rs_obj01_heading_decode(objdf.rs_obj01_heading);
    object.length = mc1_cr_rs_obj_fd_rr_rs_obj01_length_decode(objdf.rs_obj01_length);
    object.width = mc1_cr_rs_obj_fd_rr_rs_obj01_width_decode(objdf.rs_obj01_width);
    object.type = MankuObjectTypeMap[objdf.rs_obj01_type];
}

void MankuCanAdapter::fieldParsingf01(interface::msg::Object& object, mc1_fmr_rs_obj_fd_t& objdf)
{
    object.source = 2;
    object.id = objdf.rs_obj00_id;
    object.x = mc1_fmr_rs_obj_fd_rs_obj00_x_pos_decode(objdf.rs_obj00_x_pos);
    object.y = mc1_fmr_rs_obj_fd_rs_obj00_y_pos_decode(objdf.rs_obj00_y_pos);
    object.y = 0;
    object.speedx = mc1_fmr_rs_obj_fd_rs_obj00_x_vel_absolute_decode(objdf.rs_obj00_x_vel_absolute);
    object.speedy = mc1_fmr_rs_obj_fd_rs_obj00_y_vel_absolute_decode(objdf.rs_obj00_y_vel_absolute);
    object.speed = std::hypot(object.speedx, object.speedy);
    object.heading = mc1_fmr_rs_obj_fd_rs_obj00_heading_decode(objdf.rs_obj00_heading);
    object.length = mc1_fmr_rs_obj_fd_rs_obj00_length_decode(objdf.rs_obj00_length);
    object.width = mc1_fmr_rs_obj_fd_rs_obj00_width_decode(objdf.rs_obj00_width);
    object.type = MankuObjectTypeMap[objdf.rs_obj00_type];
}
void MankuCanAdapter::fieldParsingf02(interface::msg::Object& object, mc1_fmr_rs_obj_fd_t& objdf)
{
    object.source = 2;
    object.id = objdf.rs_obj00_id;
    object.x = mc1_fmr_rs_obj_fd_rs_obj01_x_pos_decode(objdf.rs_obj01_x_pos);
    object.y = mc1_fmr_rs_obj_fd_rs_obj01_y_pos_decode(objdf.rs_obj01_y_pos);
    object.y = 0;
    object.speedx = mc1_fmr_rs_obj_fd_rs_obj01_x_vel_absolute_decode(objdf.rs_obj01_x_vel_absolute);
    object.speedy = mc1_fmr_rs_obj_fd_rs_obj01_y_vel_absolute_decode(objdf.rs_obj01_y_vel_absolute);
    object.speed = std::hypot(object.speedx, object.speedy);
    object.heading = mc1_fmr_rs_obj_fd_rs_obj01_heading_decode(objdf.rs_obj01_heading);
    object.length = mc1_fmr_rs_obj_fd_rs_obj01_length_decode(objdf.rs_obj01_length);
    object.width = mc1_fmr_rs_obj_fd_rs_obj01_width_decode(objdf.rs_obj01_width);
    object.type = MankuObjectTypeMap[objdf.rs_obj01_type];
}

void MankuCanAdapter::onReceiverRS6bbFRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct mc1_cr_rs_obj_fd_fr_t objdf;
    auto piece = buffer.toStringPiece();
    int retv = mc1_cr_rs_obj_fd_fr_unpack(&objdf, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar2 [%04x] mc1_cr_rs_obj_fd_fr_unpack return %d", fid, retv);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (mc1_cr_rs_obj_fd_rl_rs_obj00_id_is_in_range(objdf.rs_obj00_id)) {
        if (radar2ObjectsMap.find(objdf.rs_obj00_id) != radar2ObjectsMap.end()) {
            object = radar2ObjectsMap[objdf.rs_obj00_id].second;
            fieldParsingfr01(object, objdf);
        } else {
            fieldParsingfr01(object, objdf);
            auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
            radar2ObjectsMap[objdf.rs_obj00_id] = obj;
        }
    }
    if (mc1_cr_rs_obj_fd_rl_rs_obj00_id_is_in_range(objdf.rs_obj01_id)) {
        if (radar2ObjectsMap.find(objdf.rs_obj01_id) != radar2ObjectsMap.end()) {
            object = radar2ObjectsMap[objdf.rs_obj01_id].second;
            fieldParsingfr02(object, objdf);
        } else {
            fieldParsingfr02(object, objdf);
            auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
            radar2ObjectsMap[objdf.rs_obj01_id] = obj;
        }
    }
}
void MankuCanAdapter::onReceiverRS6abRRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct mc1_cr_rs_obj_fd_rr_t objdf;
    auto piece = buffer.toStringPiece();
    int retv = mc1_cr_rs_obj_fd_rr_unpack(&objdf, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar2 [%04x] mc1_cr_rs_obj_fd_rr_unpack return %d", fid, retv);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (mc1_cr_rs_obj_fd_rl_rs_obj00_id_is_in_range(objdf.rs_obj00_id)) {
        if (radar3ObjectsMap.find(objdf.rs_obj00_id) != radar3ObjectsMap.end()) {
            object = radar3ObjectsMap[objdf.rs_obj00_id].second;
            fieldParsingrr01(object, objdf);
        } else {
            fieldParsingrr01(object, objdf);
            auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
            radar3ObjectsMap[objdf.rs_obj00_id] = obj;
        }
    }
    if (mc1_cr_rs_obj_fd_rl_rs_obj00_id_is_in_range(objdf.rs_obj01_id)) {
        if (radar3ObjectsMap.find(objdf.rs_obj01_id) != radar3ObjectsMap.end()) {
            object = radar3ObjectsMap[objdf.rs_obj01_id].second;
            fieldParsingrr02(object, objdf);
        } else {
            fieldParsingrr02(object, objdf);
            auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
            radar3ObjectsMap[objdf.rs_obj01_id] = obj;
        }
    }
}

void MankuCanAdapter::onReceiverRS6ebFInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    struct mc1_fmr_rs_obj_fd_t objdf;
    auto piece = buffer.toStringPiece();
    int retv = mc1_fmr_rs_obj_fd_unpack(&objdf, (const uint8_t*)(piece.data()), piece.size());
    RCLCPP_INFO(logger_, "radar0 [%04x] mc1_fmr_rs_obj_fd_unpack return %d", fid, retv);
    auto now = muduo::Timestamp::now();
    interface::msg::Object object;
    object.header.stamp.sec = now.secondsSinceEpoch();
    object.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    if (mc1_fmr_rs_obj_fd_rs_obj00_id_is_in_range(objdf.rs_obj00_id)) {
        if (radar0ObjectsMap.find(objdf.rs_obj00_id) != radar0ObjectsMap.end()) {
            object = radar0ObjectsMap[objdf.rs_obj00_id].second;
            fieldParsingf01(object, objdf);
        } else {
            fieldParsingf01(object, objdf);
            auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
            radar0ObjectsMap[objdf.rs_obj00_id] = obj;
        }
    }
    if (mc1_fmr_rs_obj_fd_rs_obj00_id_is_in_range(objdf.rs_obj01_id)) {
        if (radar0ObjectsMap.find(objdf.rs_obj01_id) != radar0ObjectsMap.end()) {
            object = radar0ObjectsMap[objdf.rs_obj01_id].second;
            fieldParsingf02(object, objdf);
        } else {
            fieldParsingf02(object, objdf);
            auto obj = std::make_pair(now.microSecondsSinceEpoch(), object);
            radar0ObjectsMap[objdf.rs_obj01_id] = obj;
        }
    }
}

void MankuCanAdapter::onReceiverRS520FRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
}
void MankuCanAdapter::onReceiverRS510RRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
}

void MankuCanAdapter::onReceiverRS551FMRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    auto now = muduo::Timestamp::now();
    std::lock_guard<std::mutex> locker(radar0MapLock_);
    uint64_t time = now.microSecondsSinceEpoch();
    interface::msg::Objects objes;
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (time % 1000000) * 1000;
    objes.header.frame_id = "radar0";
    for (auto it = radar0ObjectsMap.begin(); it != radar0ObjectsMap.end();) {
        std::cout << printObject(it->second.second) << std::endl;
        objes.objects.emplace_back(it->second.second);
        ++it;
    }
    radar_obj_pubs_[objes.header.frame_id]->publish(objes);
    radar0ObjectsMap.clear();
}
void MankuCanAdapter::onReceiverRS511RRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    auto now = muduo::Timestamp::now();
    std::lock_guard<std::mutex> locker(radar0MapLock_);
    uint64_t time = now.microSecondsSinceEpoch();
    interface::msg::Objects objes;
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (time % 1000000) * 1000;
    objes.header.frame_id = "radar3";
    for (auto it = radar3ObjectsMap.begin(); it != radar3ObjectsMap.end();) {
        std::cout << printObject(it->second.second) << std::endl;
        objes.objects.emplace_back(it->second.second);
        ++it;
    }
    radar_obj_pubs_[objes.header.frame_id]->publish(objes);
    radar3ObjectsMap.clear();
}
void MankuCanAdapter::onReceiverRS521FRInfo(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    auto now = muduo::Timestamp::now();
    std::lock_guard<std::mutex> locker(radar0MapLock_);
    uint64_t time = now.microSecondsSinceEpoch();
    interface::msg::Objects objes;
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (time % 1000000) * 1000;
    objes.header.frame_id = "radar2";
    for (auto it = radar2ObjectsMap.begin(); it != radar2ObjectsMap.end();) {
        std::cout << printObject(it->second.second) << std::endl;
        objes.objects.emplace_back(it->second.second);
        ++it;
    }
    radar_obj_pubs_[objes.header.frame_id]->publish(objes);
    radar2ObjectsMap.clear();
}

std::string MankuCanAdapter::printObject(const interface::msg::Object& object)
{
    std::stringstream ss;

    ss << "frameId:" << object.header.frame_id << ",id:" << object.id << ",x:" << object.x << ",y:" << object.y << ",type:" << object.type
       << ",heading:" << object.heading << ",speed:" << object.speed << ",speedX:" << object.speedx << ",speedY:" << object.speedy;

    return ss.str();
}